package math.base;

import java.util.Arrays;

/**
 * @author Saga
 *
 */
public class Matrix3d {
    public Matrix3d() {
        this.elements = new double[]{
                1.0, 0.0, 0.0,
                0.0, 1.0, 0.0,
                0.0, 0.0, 1.0 };
    }

    public Matrix3d(final double _a11, final double _a12, final double _a13, final double _a21,
            final double _a22, final double _a23, final double _a31, final double _a32,
            final double _a33) {
        this.elements = new double[]{ _a11, _a21, _a31, _a12, _a22, _a32, _a13, _a23, _a33 };
    }

    public Matrix3d(final Coord3d _col1, final Coord3d _col2, final Coord3d _col3) {
        this.elements =
                new double[]{ _col1.getX(), _col1.getY(), _col1.getZ(),
                        _col2.getX(), _col2.getY(), _col2.getZ(),
                        _col3.getX(), _col3.getY(), _col3.getZ() };
    }

    public Matrix3d(final AxisAngle _axisAngle) {
        final double cosVal = Math.cos(_axisAngle.getAngle());
        final double comCosVal = 1.0 - cosVal;
        final double sinVal = Math.sin(_axisAngle.getAngle());

        final double xy = _axisAngle.getAxis().getX() * _axisAngle.getAxis().getY();
        final double yz = _axisAngle.getAxis().getY() * _axisAngle.getAxis().getZ();
        final double zx = _axisAngle.getAxis().getZ() * _axisAngle.getAxis().getX();

        final double xx = _axisAngle.getAxis().getX() * _axisAngle.getAxis().getX();
        final double yy = _axisAngle.getAxis().getY() * _axisAngle.getAxis().getY();
        final double zz = _axisAngle.getAxis().getZ() * _axisAngle.getAxis().getZ();

        final double x = _axisAngle.getAxis().getX();
        final double y = _axisAngle.getAxis().getY();
        final double z = _axisAngle.getAxis().getZ();

        this.elements = new double[]{ cosVal + comCosVal * xx, sinVal * z + comCosVal * xy,
                -sinVal * y + comCosVal * zx, -sinVal * z + comCosVal * xy, cosVal + comCosVal * yy,
                sinVal * z + comCosVal * yz, sinVal * y + comCosVal * zx,
                -sinVal * _axisAngle.getAxis().getX() + comCosVal * yz, cosVal + zz * comCosVal };
    }

    //coord
    public final void setCoord(final int _rowId, final int _colId, final double _val) {
        switch (_rowId) {
        case 0: {
            switch (_colId) {
            case 0:
                this.elements[0] = _val;
                break;

            case 1:
                this.elements[3] = _val;
                break;

            case 2:
                this.elements[6] = _val;
                break;

            default:
                throw new IndexOutOfBoundsException("");
            }
        }
        break;

        case 1: {
            switch (_colId) {
            case 0:
                this.elements[1] = _val;
                break;

            case 1:
                this.elements[4] = _val;
                break;

            case 2:
                this.elements[7] = _val;
                break;

            default:
                throw new IndexOutOfBoundsException("");
            }
        }
        break;

        case 2: {
            switch (_colId) {
            case 0:
                this.elements[2] = _val;
                break;

            case 1:
                this.elements[5] = _val;
                break;

            case 2:
                this.elements[8] = _val;
                break;

            default:
                throw new IndexOutOfBoundsException("");
            }
        }
        break;

        default:
            throw new IndexOutOfBoundsException("");
        }
    }

    public final double getCoord(final int _rowId, final int _colId) {
        switch (_rowId) {
        case 0: {
            switch (_colId) {
            case 0:
                return this.elements[0];

            case 1:
                return this.elements[3];

            case 2:
                return this.elements[6];

            default:
                throw new IndexOutOfBoundsException("");
            }
        }

        case 1: {
            switch (_colId) {
            case 0:
                return this.elements[1];

            case 1:
                return this.elements[4];

            case 2:
                return this.elements[7];

            default:
                throw new IndexOutOfBoundsException("");
            }
        }

        case 2: {
            switch (_colId) {
            case 0:
                return this.elements[2];

            case 1:
                return this.elements[5];

            case 2:
                return this.elements[8];

            default:
                throw new IndexOutOfBoundsException("");
            }
        }

        default:
            throw new IndexOutOfBoundsException("");
        }
    }

    public final void setRow(final int _id, final Coord3d _row) {
        if (_id < 0 || _id > 2) {
            throw new IndexOutOfBoundsException("");
        }

        this.elements[_id] = _row.getX();
        this.elements[_id + 3] = _row.getY();
        this.elements[_id + 6] = _row.getZ();
    }

    public final Coord3d getRow(final int _id) {
        if (_id < 0 || _id > 2) {
            throw new IndexOutOfBoundsException("");
        }

        return new Coord3d(this.elements[_id], this.elements[_id + 3], this.elements[_id + 6]);
    }

    public final void setColumn(final int _id, final Coord3d _col) {
        if (_id < 0 || _id > 2) {
            throw new IndexOutOfBoundsException("");
        }

        final int pos = _id * 3;
        this.elements[pos] = _col.getX();
        this.elements[pos + 1] = _col.getY();
        this.elements[pos + 2] = _col.getZ();
    }

    public final Coord3d getColumn(final int _id) {
        if (_id < 0 || _id > 2) {
            throw new IndexOutOfBoundsException("");
        }

        final int pos = _id * 3;
        return new Coord3d(this.elements[pos], this.elements[pos + 1], this.elements[pos + 2]);
    }

    public final void setIdentity() {
        this.elements[0] = 1.0;
        this.elements[1] = 0.0;
        this.elements[2] = 0.0;
        this.elements[3] = 0.0;
        this.elements[4] = 1.0;
        this.elements[5] = 0.0;
        this.elements[6] = 0.0;
        this.elements[7] = 0.0;
        this.elements[8] = 1.0;
    }

    public static final Matrix3d createFromIdentity() {
        return new Matrix3d(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
    }

    public final void setZero() {
        Arrays.fill(this.elements, 0.0);
    }

    public static final Matrix3d createFromZero() {
        return new Matrix3d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    }

    public final void setConstant(final double _val) {
        Arrays.fill(this.elements, _val);
    }

    public static final Matrix3d createFromConstant(final double _val) {
        return new Matrix3d(_val, _val, _val, _val, _val, _val, _val, _val, _val);
    }

    public final void setDiagonal(final double _val) {
        this.elements[0] = _val;
        this.elements[1] = 0.0;
        this.elements[2] = 0.0;
        this.elements[3] = 0.0;
        this.elements[4] = _val;
        this.elements[5] = 0.0;
        this.elements[6] = 0.0;
        this.elements[7] = 0.0;
        this.elements[8] = _val;
    }

    public static final Matrix3d createFromDiagonal(final double _val) {
        return new Matrix3d(_val, 0.0, 0.0, 0.0, _val, 0.0, 0.0, 0.0, _val);
    }

    public final void setDiagonal(final Coord3d _coord) {
        this.elements[0] = _coord.getX();
        this.elements[1] = 0.0;
        this.elements[2] = 0.0;
        this.elements[3] = 0.0;
        this.elements[4] = _coord.getY();
        this.elements[5] = 0.0;
        this.elements[6] = 0.0;
        this.elements[7] = 0.0;
        this.elements[8] = _coord.getZ();
    }

    public static final Matrix3d createFromDiagonal(final Coord3d _coord) {
        return new Matrix3d(_coord.getX(), 0.0, 0.0, 0.0, _coord.getY(), 0.0, 0.0, 0.0,
                _coord.getZ());
    }

    //add
    public final void add(final Matrix3d _mat) {
        this.elements[0] += _mat.elements[0];
        this.elements[1] += _mat.elements[1];
        this.elements[2] += _mat.elements[2];
        this.elements[3] += _mat.elements[3];
        this.elements[4] += _mat.elements[4];
        this.elements[5] += _mat.elements[5];
        this.elements[6] += _mat.elements[6];
        this.elements[7] += _mat.elements[7];
        this.elements[8] += _mat.elements[8];
    }

    public final Matrix3d added(final Matrix3d _mat) {
        return new Matrix3d(this.elements[0] + _mat.elements[0],
                this.elements[3] + _mat.elements[3], this.elements[6] + _mat.elements[6],
                this.elements[1] + _mat.elements[1], this.elements[4] + _mat.elements[4],
                this.elements[7] + _mat.elements[7], this.elements[2] + _mat.elements[2],
                this.elements[5] + _mat.elements[5], this.elements[8] + _mat.elements[8]);
    }

    //subtract
    public final void subtract(final Matrix3d _mat) {
        this.elements[0] -= _mat.elements[0];
        this.elements[1] -= _mat.elements[1];
        this.elements[2] -= _mat.elements[2];
        this.elements[3] -= _mat.elements[3];
        this.elements[4] -= _mat.elements[4];
        this.elements[5] -= _mat.elements[5];
        this.elements[6] -= _mat.elements[6];
        this.elements[7] -= _mat.elements[7];
        this.elements[8] -= _mat.elements[8];
    }

    public final Matrix3d subtracted(final Matrix3d _mat) {
        return new Matrix3d(this.elements[0] - _mat.elements[0],
                this.elements[3] - _mat.elements[3], this.elements[6] - _mat.elements[6],
                this.elements[1] - _mat.elements[1], this.elements[4] - _mat.elements[4],
                this.elements[7] - _mat.elements[7], this.elements[2] - _mat.elements[2],
                this.elements[5] - _mat.elements[5], this.elements[8] - _mat.elements[8]);
    }

    //reverse
    public final void reverse() {
        this.elements[0] = -this.elements[0];
        this.elements[1] = -this.elements[1];
        this.elements[2] = -this.elements[2];
        this.elements[3] = -this.elements[3];
        this.elements[4] = -this.elements[4];
        this.elements[5] = -this.elements[5];
        this.elements[6] = -this.elements[6];
        this.elements[7] = -this.elements[7];
        this.elements[8] = -this.elements[8];
    }

    public final Matrix3d reversed() {
        return new Matrix3d(-this.elements[0], -this.elements[3], -this.elements[6],
                -this.elements[1], -this.elements[4], -this.elements[7], -this.elements[2],
                -this.elements[5], -this.elements[8]);
    }

    //multiply
    public final void multiply(final double _scalar) {
        this.elements[0] *= _scalar;
        this.elements[1] *= _scalar;
        this.elements[2] *= _scalar;
        this.elements[3] *= _scalar;
        this.elements[4] *= _scalar;
        this.elements[5] *= _scalar;
        this.elements[6] *= _scalar;
        this.elements[7] *= _scalar;
        this.elements[8] *= _scalar;
    }

    public final Matrix3d multiplied(final double _scalar) {
        return new Matrix3d(this.elements[0] * _scalar, this.elements[3] * _scalar,
                this.elements[6] * _scalar, this.elements[1] * _scalar, this.elements[4] * _scalar,
                this.elements[7] * _scalar, this.elements[2] * _scalar, this.elements[5] * _scalar,
                this.elements[8] * _scalar);
    }

    public final void multiply(final Matrix3d _mat) {
        final double temp11 =
                this.elements[0] * _mat.elements[0] + this.elements[3] * _mat.elements[1] +
                        this.elements[6] * _mat.elements[2];
        final double temp12 =
                this.elements[0] * _mat.elements[3] + this.elements[3] * _mat.elements[4] +
                        this.elements[6] * _mat.elements[5];
        final double temp13 =
                this.elements[0] * _mat.elements[6] + this.elements[3] * _mat.elements[7] +
                        this.elements[6] * _mat.elements[8];

        final double temp21 =
                this.elements[1] * _mat.elements[0] + this.elements[4] * _mat.elements[1] +
                        this.elements[7] * _mat.elements[2];
        final double temp22 =
                this.elements[1] * _mat.elements[3] + this.elements[4] * _mat.elements[4] +
                        this.elements[7] * _mat.elements[5];
        final double temp23 =
                this.elements[1] * _mat.elements[6] + this.elements[4] * _mat.elements[7] +
                        this.elements[7] * _mat.elements[8];

        final double temp31 =
                this.elements[2] * _mat.elements[0] + this.elements[5] * _mat.elements[1] +
                        this.elements[8] * _mat.elements[2];
        final double temp32 =
                this.elements[2] * _mat.elements[3] + this.elements[5] * _mat.elements[4] +
                        this.elements[8] * _mat.elements[5];
        this.elements[8] =
                this.elements[2] * _mat.elements[6] + this.elements[5] * _mat.elements[7] +
                        this.elements[8] * _mat.elements[8];

        this.elements[0] = temp11;
        this.elements[1] = temp21;
        this.elements[2] = temp31;
        this.elements[3] = temp12;
        this.elements[4] = temp22;
        this.elements[5] = temp32;
        this.elements[6] = temp13;
        this.elements[7] = temp23;
    }

    public final Matrix3d multiplied(final Matrix3d _mat) {
        final double temp11 =
                this.elements[0] * _mat.elements[0] + this.elements[3] * _mat.elements[1] +
                        this.elements[6] * _mat.elements[2];
        final double temp12 =
                this.elements[0] * _mat.elements[3] + this.elements[3] * _mat.elements[4] +
                        this.elements[6] * _mat.elements[5];
        final double temp13 =
                this.elements[0] * _mat.elements[6] + this.elements[3] * _mat.elements[7] +
                        this.elements[6] * _mat.elements[8];

        final double temp21 =
                this.elements[1] * _mat.elements[0] + this.elements[4] * _mat.elements[1] +
                        this.elements[7] * _mat.elements[2];
        final double temp22 =
                this.elements[1] * _mat.elements[3] + this.elements[4] * _mat.elements[4] +
                        this.elements[7] * _mat.elements[5];
        final double temp23 =
                this.elements[1] * _mat.elements[6] + this.elements[4] * _mat.elements[7] +
                        this.elements[7] * _mat.elements[8];

        final double temp31 =
                this.elements[2] * _mat.elements[0] + this.elements[5] * _mat.elements[1] +
                        this.elements[8] * _mat.elements[2];
        final double temp32 =
                this.elements[2] * _mat.elements[3] + this.elements[5] * _mat.elements[4] +
                        this.elements[8] * _mat.elements[5];
        final double temp33 =
                this.elements[2] * _mat.elements[6] + this.elements[5] * _mat.elements[7] +
                        this.elements[8] * _mat.elements[8];

        return new Matrix3d(temp11, temp12, temp13, temp21, temp22, temp23, temp31, temp32, temp33);

    }

    public final void preMultiply(final Matrix3d _mat) {
        final double temp11 =
                _mat.elements[0] * this.elements[0] + _mat.elements[3] * this.elements[1] +
                        _mat.elements[6] * this.elements[2];
        final double temp12 =
                _mat.elements[0] * this.elements[3] + _mat.elements[3] * this.elements[4] +
                        _mat.elements[6] * this.elements[5];
        final double temp13 =
                _mat.elements[0] * this.elements[6] + _mat.elements[3] * this.elements[7] +
                        _mat.elements[6] * this.elements[8];

        final double temp21 =
                _mat.elements[1] * this.elements[0] + _mat.elements[4] * this.elements[1] +
                        _mat.elements[7] * this.elements[2];
        final double temp22 =
                _mat.elements[1] * this.elements[3] + _mat.elements[4] * this.elements[4] +
                        _mat.elements[7] * this.elements[5];
        final double temp23 =
                _mat.elements[1] * this.elements[6] + _mat.elements[4] * this.elements[7] +
                        _mat.elements[7] * this.elements[8];

        final double temp31 =
                _mat.elements[2] * this.elements[0] + _mat.elements[5] * this.elements[1] +
                        _mat.elements[8] * this.elements[2];
        final double temp32 =
                _mat.elements[2] * this.elements[3] + _mat.elements[5] * this.elements[4] +
                        _mat.elements[8] * this.elements[5];
        this.elements[8] =
                _mat.elements[2] * this.elements[6] + _mat.elements[5] * this.elements[7] +
                        _mat.elements[8] * this.elements[8];

        this.elements[0] = temp11;
        this.elements[1] = temp21;
        this.elements[2] = temp31;
        this.elements[3] = temp12;
        this.elements[4] = temp22;
        this.elements[5] = temp32;
        this.elements[6] = temp13;
        this.elements[7] = temp23;
    }

    public final Matrix3d preMultiplied(final Matrix3d _mat) {
        final double temp11 =
                _mat.elements[0] * this.elements[0] + _mat.elements[3] * this.elements[1] +
                        _mat.elements[6] * this.elements[2];
        final double temp12 =
                _mat.elements[0] * this.elements[3] + _mat.elements[3] * this.elements[4] +
                        _mat.elements[6] * this.elements[5];
        final double temp13 =
                _mat.elements[0] * this.elements[6] + _mat.elements[3] * this.elements[7] +
                        _mat.elements[6] * this.elements[8];

        final double temp21 =
                _mat.elements[1] * this.elements[0] + _mat.elements[4] * this.elements[1] +
                        _mat.elements[7] * this.elements[2];
        final double temp22 =
                _mat.elements[1] * this.elements[3] + _mat.elements[4] * this.elements[4] +
                        _mat.elements[7] * this.elements[5];
        final double temp23 =
                _mat.elements[1] * this.elements[6] + _mat.elements[4] * this.elements[7] +
                        _mat.elements[7] * this.elements[8];

        final double temp31 =
                _mat.elements[2] * this.elements[0] + _mat.elements[5] * this.elements[1] +
                        _mat.elements[8] * this.elements[2];
        final double temp32 =
                _mat.elements[2] * this.elements[3] + _mat.elements[5] * this.elements[4] +
                        _mat.elements[8] * this.elements[5];
        final double temp33 =
                _mat.elements[2] * this.elements[6] + _mat.elements[5] * this.elements[7] +
                        _mat.elements[8] * this.elements[8];

        return new Matrix3d(temp11, temp12, temp13, temp21, temp22, temp23, temp31, temp32, temp33);
    }

    public final Coord3d multiplied(final Coord3d _coord) {
        final double tempX = this.elements[0] * _coord.getX() + this.elements[3] * _coord.getY() +
                this.elements[6] * _coord.getZ();
        final double tempY = this.elements[1] * _coord.getX() + this.elements[4] * _coord.getY() +
                this.elements[7] * _coord.getZ();
        final double tempZ = this.elements[2] * _coord.getX() + this.elements[5] * _coord.getY() +
                this.elements[8] * _coord.getZ();

        return new Coord3d(tempX, tempY, tempZ);
    }

    public final Coord3d preMultiplied(final Coord3d _coord) {
        final double tempX = this.elements[0] * _coord.getX() + this.elements[1] * _coord.getY() +
                this.elements[2] * _coord.getZ();
        final double tempY = this.elements[3] * _coord.getX() + this.elements[4] * _coord.getY() +
                this.elements[5] * _coord.getZ();
        final double tempZ = this.elements[6] * _coord.getX() + this.elements[7] * _coord.getY() +
                this.elements[8] * _coord.getZ();

        return new Coord3d(tempX, tempY, tempZ);
    }

    //divide
    public final void divide(final double _denom) {
        final double scalar = 1.0 / _denom;
        this.elements[0] *= scalar;
        this.elements[1] *= scalar;
        this.elements[2] *= scalar;
        this.elements[3] *= scalar;
        this.elements[4] *= scalar;
        this.elements[5] *= scalar;
        this.elements[6] *= scalar;
        this.elements[7] *= scalar;
        this.elements[8] *= scalar;
    }

    public final Matrix3d divided(final double _denom) {
        final double scalar = 1.0 / _denom;
        return new Matrix3d(this.elements[0] * scalar, this.elements[3] * scalar,
                this.elements[6] * scalar, this.elements[1] * scalar, this.elements[4] * scalar,
                this.elements[7] * scalar, this.elements[2] * scalar, this.elements[5] * scalar,
                this.elements[8] * scalar);
    }

    //tranpose
    public final void transpose() {
        double temp = this.elements[1];
        this.elements[1] = this.elements[3];
        this.elements[3] = temp;

        temp = this.elements[5];
        this.elements[5] = this.elements[7];
        this.elements[7] = temp;

        temp = this.elements[2];
        this.elements[2] = this.elements[6];
        this.elements[6] = temp;
    }

    public final Matrix3d transposed() {
        return new Matrix3d(this.elements[0], this.elements[1], this.elements[2], this.elements[3],
                this.elements[4], this.elements[5], this.elements[6], this.elements[7],
                this.elements[8]);
    }

    //invert
    public final void invert() {
        final double temp11 =
                this.elements[4] * this.elements[8] - this.elements[5] * this.elements[7];
        final double temp21 =
                this.elements[5] * this.elements[6] - this.elements[3] * this.elements[8];
        final double temp31 =
                this.elements[3] * this.elements[7] - this.elements[4] * this.elements[6];

        final double det =
                this.elements[0] * temp11 + this.elements[1] * temp21 + this.elements[2] * temp31;
        if (Math.abs(det) == 0.0) {
            throw new ArithmeticException("");
        }

        final double temp12 =
                this.elements[2] * this.elements[7] - this.elements[1] * this.elements[8];
        final double temp22 =
                this.elements[0] * this.elements[8] - this.elements[2] * this.elements[6];
        final double temp32 =
                this.elements[1] * this.elements[6] - this.elements[0] * this.elements[7];

        final double temp13 =
                this.elements[1] * this.elements[5] - this.elements[2] * this.elements[4];
        final double temp23 =
                this.elements[2] * this.elements[3] - this.elements[0] * this.elements[5];
        final double temp33 =
                this.elements[0] * this.elements[4] - this.elements[1] * this.elements[3];

        final double scalar = 1.0 / det;
        this.elements[0] = temp11 * scalar;
        this.elements[1] = temp12 * scalar;
        this.elements[2] = temp13 * scalar;
        this.elements[3] = temp21 * scalar;
        this.elements[4] = temp22 * scalar;
        this.elements[5] = temp23 * scalar;
        this.elements[6] = temp31 * scalar;
        this.elements[7] = temp32 * scalar;
        this.elements[8] = temp33 * scalar;
    }

    public final Matrix3d inverted() {
        final double temp11 =
                this.elements[4] * this.elements[8] - this.elements[5] * this.elements[7];
        final double temp21 =
                this.elements[5] * this.elements[6] - this.elements[3] * this.elements[8];
        final double temp31 =
                this.elements[3] * this.elements[7] - this.elements[4] * this.elements[6];

        final double det =
                this.elements[0] * temp11 + this.elements[1] * temp21 + this.elements[2] * temp31;
        if (Math.abs(det) == 0.0) {
            throw new ArithmeticException("");
        }

        final double temp12 =
                this.elements[2] * this.elements[7] - this.elements[1] * this.elements[8];
        final double temp22 =
                this.elements[0] * this.elements[8] - this.elements[2] * this.elements[6];
        final double temp32 =
                this.elements[1] * this.elements[6] - this.elements[0] * this.elements[7];

        final double temp13 =
                this.elements[1] * this.elements[5] - this.elements[2] * this.elements[4];
        final double temp23 =
                this.elements[2] * this.elements[3] - this.elements[0] * this.elements[5];
        final double temp33 =
                this.elements[0] * this.elements[4] - this.elements[1] * this.elements[3];

        final double scalar = 1.0 / det;
        return new Matrix3d(temp11 * scalar, temp21 * scalar, temp31 * scalar, temp12 * scalar,
                temp22 * scalar, temp32 * scalar, temp13 * scalar, temp23 * scalar,
                temp33 * scalar);
    }

    public final void power(final int _order) {
        switch (_order) {
        case 1:
            return;

        case 0:
            this.setIdentity();
            return;

        case -1:
            this.invert();
            return;

        default: {
            if (_order < 0) {
                this.invert();
            }

            int tempOrder = _order;
            if (tempOrder < 0) {
                tempOrder = -tempOrder;
            }
            --tempOrder;

            final Matrix3d temp = this.clone();
            for (; ; ) {
                if (tempOrder % 2 == 1) {
                    this.multiply(temp);
                }
                if (tempOrder == 1) {
                    return;
                }
                temp.multiply(temp);
                tempOrder >>= 1;
            }
        }
        }
    }

    public final Matrix3d powered(final int _order) {
        switch (_order) {
        case 1:
            return this.clone();

        case 0:
            return createFromIdentity();

        case -1:
            return this.inverted();

        default: {
            Matrix3d result = this.clone();
            if (_order < 0) {
                result = this.inverted();
            }

            int tempOrder = _order;
            if (tempOrder < 0) {
                tempOrder = -tempOrder;
            }
            --tempOrder;

            final Matrix3d temp = result.clone();
            for (; ; ) {
                if (tempOrder % 2 == 1) {
                    result.multiply(temp);
                }
                if (tempOrder == 1) {
                    return result;
                }
                temp.multiply(temp);
                tempOrder >>= 1;
            }
        }
        }
    }

    //attribute
    public final double calcTrace() {
        return this.elements[0] + this.elements[4] + this.elements[8];
    }

    public final double calcDeterminant() {
        return this.elements[0] *
                (this.elements[4] * this.elements[8] - this.elements[5] * this.elements[7]) +
                this.elements[1] * (this.elements[5] * this.elements[6] -
                        this.elements[3] * this.elements[8]) + this.elements[2] *
                (this.elements[3] * this.elements[7] - this.elements[4] * this.elements[6]);
    }

    @Override
    public final Matrix3d clone() {
        return new Matrix3d(this.elements[0], this.elements[3], this.elements[6], this.elements[1],
                this.elements[4], this.elements[7], this.elements[2], this.elements[5],
                this.elements[8]);
    }

    public double[] elements;
}
